#!/usr/bin/python
# -*- coding: UTF-8 -*-

from MiniBotControlLib import MiniBotControlLib
import time

# ------------------------------
# 	2.1、连接设备 Connect
#   2.4、扫描模块ID ScanId
#   2.8、设置关节模块模式 SetMotorMode
#   2.18、设置关节模块是否使能 EnableMotor
#   2.19、设置关节模块是否锁定 SetMotorLocked
#   2.20、设置模块舵机参数 SetMotorParamter
#   2.21、获取模块舵机参数 GetMotorParamter
# 	6、断开设备 Disconnect
# ------------------------------

# 声明控制库对象
mlib = MiniBotControlLib("udp")


# *************************
#   2.1、连接设备
#*************************#
ip = "10.10.100.254"  # "10.10.200.254"
r = mlib.Connect(ip, 8899)
# 带超时的连接方式
#r = mlib.Connect(ip,8899,2000)
if(r > 0):
    print("连接成功")
else:
    print("连接失败")

# *************************
#   2.4、扫描模块ID
# *************************
motorIds, motorLocs = mlib.ScanId(0, 50)
print("motorIds", motorIds)
print("motorLocs", motorLocs)


# *************************
#   2.18、设置关节模块是否使能 EnableMotor
# *************************
input("输入任意字符回车，设置关节模块使能-开启, 此时模块处于上电模式，手掰不动")
for id in motorIds:
    r = mlib.EnableMotor(id, True)
    if(r):
        print(str(id) + "设置成功")
    else:
        print(str(id) + "设置失败")
    time.sleep(0.5)

input("输入任意字符回车，设置关节模块使能-关闭，此时模块可以手掰动")
for id in motorIds:
    r = mlib.EnableMotor(id, False)
    if(r):
        print(str(id) + "设置成功")
    else:
        print(str(id) + "设置失败")
    time.sleep(0.5)

# *************************
#    2.19、设置关节模块是否锁定
# *************************
input("输入任意字符回车，设置关节模块是否锁定-关闭")
for id in motorIds:
    r = mlib.SetMotorLocked(id, 0)
    if(r):
        print(str(id) + "设置成功")
    else:
        print(str(id) + "设置失败")
    time.sleep(0.5)

# *************************
#   2.8、设置关节模块模式
# *************************
input("输入任意字符回车，设置模块为位置模式")
id = motorIds[0]
r = mlib.SetMotorMode(id, 0)
if(r):
    print(str(id) + "设置成功")
else:
    print(str(id) + "设置失败")


# *************************
#   2.20、设置模块舵机参数
# *************************
input("输入任意字符回车， 设置模块"+str(id)+"舵机参数-目标位置")
r = mlib.SetMotorParamter(id, 0x2A, 2, 2048)
if(r):
    print(str(id) + "设置成功")
else:
    print(str(id) + "设置失败")


# *************************
#   2.21、获取模块舵机参数
# *************************
input("输入任意字符回车，获取模块舵机"+str(id)+"参数-目标位置")
cpos = mlib.GetMotorParamter(id, 0x2A, 2)
print(str(id)+"当前位置"+str(cpos))


# *************************
# 	断开设备
# *************************
input("输入任意字符回车，断开连接")
r = mlib.Disconnect()
if(r > 0):
    print("断开成功")
else:
    print("断开失败")
